
import sys
from enum import Enum

class cartesianPos:
    def __init__(self,x,y,z,rx,ry,rz):
        self.x = x
        self.y = y
        self.z = z
        self.rx = rx
        self.ry = ry
        self.rz = rz

class Move(Enum):
    MOVE_L = 0
    MOVE_J = 1
    OFFSET_W = 2
    OFFSET_T =3

class Info(Enum):
    PING_OK = 0
    TCP_OK = 1
    TCP_ERR = 2
    SERVER_IS_RUNNING = 3
    SERVER_NOT_RUNNING =4
    MOVE_OK = 4
    MOVE_ERR = 5
    FRAME_ERR = 6
    RERUN_FILE_ERR = 7
    L_PLACEHOLDER_ERR = 8 
    R_PLACEHOLDER_ERR = 9
    B_PLACEHOLDER_ERR = 10
    RERUN_FILE_NAMES =11
    L_POS_RECHABLE = 12
    L_POS_UNRECHABLE = 13
    R_POS_RECHABLE = 14
    R_POS_UNRECHABLE = 15
class rlisYumi:

    _emptyPath = "null"
    
    @classmethod
    def emptyPath(cls):
        return cls._emptyPath

    #Ping
    @classmethod
    def ping(cls):
        return b"%*0*#\0"

    #PingOK
    @classmethod
    def pingOK(cls):
        return b"%*0*1*#\0"

    #检查服务器连接机器人的TCP是否OK
    @classmethod
    def checkServerTcps(cls):
        return b"%*1*#\0"
    
    #服务端连接机器人的TCP OK
    @classmethod
    def serverTcpsOK(cls):
        return b"%*1*1*#\0"

    #服务端连接机器人的TCP Error
    @classmethod
    def serverTcpsErr(cls):
        return b"%*1*0*#\0"
    
    #检查远程是否正在运行
    @classmethod
    def checkServerRunning(cls):
        return b"%*2*#\0"
    
    #远程未运行
    @classmethod
    def serverNotRunning(cls):
        return b"%*2*1*#\0"

    #远程在运行
    @classmethod
    def serverIsRunning(cls):
        return b"%*2*0*#\0"

    #左臂单独MoveL
    '''
    name    type
    left: cartesianPos
    rerunPath: string
    '''
    @classmethod
    def LMoveL(cls,left,rerunPath):
        msg = "%*3*MoveL,"+str(left.x) + ","+\
                           str(left.y) + ","+\
                           str(left.z) + ","+\
                           str(left.rx) + ","+\
                           str(left.ry) + ","+\
                           str(left.rz) + ",L*"+\
                           rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg


    #左臂单独MoveJ
    '''
    name    type
    left: cartesianPos
    rerunPath: string
    '''
    @classmethod
    def LMoveJ(cls,left,rerunPath):
        msg = "%*3*MoveJ,"+str(left.x) + ","+\
                           str(left.y) + ","+\
                           str(left.z) + ","+\
                           str(left.rx) + ","+\
                           str(left.ry) + ","+\
                           str(left.rz) + ",L*"+\
                           rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg
    
    #左臂单独LOffsetW
    '''
    name    type
    left: cartesianPos
    rerunPath: string
    '''
    @classmethod
    def LOffsetW(cls,left,rerunPath):
        msg = "%*3*OffsetW,"+str(left.x) + ","+\
                           str(left.y) + ","+\
                           str(left.z) + ",L*"+\
                           rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg
    
    #左臂单独LOffsetT
    '''
    name    type
    left: cartesianPos
    rerunPath: string
    '''
    @classmethod
    def LOffsetT(cls,left,rerunPath):
        msg = "%*3*OffsetT,"+str(left.x) + ","+\
                    str(left.y) + ","+\
                    str(left.z) + ","+\
                    str(left.rx) + ","+\
                    str(left.ry) + ","+\
                    str(left.rz) + ",L*"+\
                    rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg

    #左臂单独ContactL
    '''
    name    type
    left: cartesianPos  相对
    rerunPath: string
    '''
    @classmethod
    def LContactL(cls,left,rerunPath):
        msg = "%*3*ContactL,"+str(left.x) + ","+\
                    str(left.y) + ","+\
                    str(left.z) + ","+\
                    str(left.rx) + ","+\
                    str(left.ry) + ","+\
                    str(left.rz) + ",L*"+\
                    rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg

    #左臂单独ContactL
    '''
    name    type
    left: cartesianPos  相对
    desireTorque: float
    rerunPath: string
    '''
    @classmethod
    def LContactL_DesireTorque(cls,left,desireTorque,rerunPath):
        msg = "%*3*ContactL,"+str(left.x) + ","+\
                    str(left.y) + ","+\
                    str(left.z) + ","+\
                    str(left.rx) + ","+\
                    str(left.ry) + ","+\
                    str(left.rz) + ","+\
                    str(desireTorque) + ",L*"+\
                    rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg

    #左臂抓手打开
    @classmethod
    def LHandOpen(cls):
        return b"%*4*HandOpen,L*#\0"

    #左臂抓手关闭
    @classmethod
    def LHandClose(cls):
        return b"%*4*HandClose,L*#\0"
    
    @classmethod
    def LHandDo0open(cls):
        return b"%*4*DO0_ON,L*#\0"

    @classmethod
    def LHandDo0close(cls):
        return b"%*4*DO0_OFF,L*#\0"

    @classmethod
    def LHandRoate(cls):
        return b"%*4*Rotate,-40.14,-72.35,-0.70,84.30,118.52,3.66,-1.07,L*#\0"

    @classmethod
    def RHandRoate(cls):
        return b"%*6*Rotate,28.67,-67.79,-19.64,-79.44,117.26,-19.22,-1.01,R*#\0"


    #右臂运动MoveL
    @classmethod
    def RMoveL(cls,right,rerunPath):
        msg = "%*5*MoveL,"+str(right.x) + ","+\
                           str(right.y) + ","+\
                           str(right.z) + ","+\
                           str(right.rx) + ","+\
                           str(right.ry) + ","+\
                           str(right.rz) + ",R*"+\
                           rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg

    #右臂运动MoveJ
    @classmethod 
    def RMoveJ(cls,right,rerunPath):
        msg = "%*5*MoveJ,"+str(right.x) + ","+\
                           str(right.y) + ","+\
                           str(right.z) + ","+\
                           str(right.rx) + ","+\
                           str(right.ry) + ","+\
                           str(right.rz) + ",R*"+\
                           rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg
    
    #右臂运动OffsetW
    @classmethod
    def ROffsetW(cls,right,rerunPath):
        msg = "%*5*OffsetW,"+str(right.x) + ","+\
                             str(right.y) + ","+\
                             str(right.z) + ",R*"+\
                             rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg
    
    #右臂运动OffsetT
    @classmethod
    def ROffsetT(cls,right,rerunPath):
        msg = "%*5*OffsetT,"+str(right.x) + ","+\
                             str(right.y) + ","+\
                             str(right.z) + ","+\
                             str(right.rx) + ","+\
                             str(right.ry) + ","+\
                             str(right.rz) + ",R*"+\
                             rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg

    #右臂单独ContactL
    '''
    name    type
    right: cartesianPos 相对
    rerunPath: string
    '''
    @classmethod
    def RContactL(cls,right,rerunPath):
        msg = "%*5*ContactL,"+str(right.x) + ","+\
                    str(right.y) + ","+\
                    str(right.z) + ","+\
                    str(right.rx) + ","+\
                    str(right.ry) + ","+\
                    str(right.rz) + ",R*"+\
                    rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg

    #右臂单独ContactL
    '''
    name    type
    right: cartesianPos  相对
    desireTorque: float 
    rerunPath: string
    '''
    @classmethod
    def RContactL_DesireTorque(cls,right,desireTorque,rerunPath):
        msg = "%*5*ContactL,"+str(right.x) + ","+\
                    str(right.y) + ","+\
                    str(right.z) + ","+\
                    str(right.rx) + ","+\
                    str(right.ry) + ","+\
                    str(right.rz) + ","+\
                    str(desireTorque) + ",R*"+\
                    rerunPath + "*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg

    #右臂抓手打开
    @classmethod
    def RHandOpen(cls):
        return b"%*6*HandOpen,R*#\0"
    
    #右臂抓手关闭
    @classmethod
    def RHandClose(cls):
        return b"%*6*HandClose,R*#\0"
    
    #左右臂运行
    @classmethod
    def LRMove(cls,left,leftMv,right,rightMv,rerunPath):
        leftCMD = ""
        rightCMD = ""
        if leftMv == Move.MOVE_J:
            leftCMD = "MoveJ,"+str(left.x) + ","+\
                               str(left.y) + ","+\
                               str(left.z) + ","+\
                               str(left.rx) + ","+\
                               str(left.ry) + ","+\
                               str(left.rz) + ",L"
        elif leftMv == Move.MOVE_L:
            leftCMD = "MoveL,"+str(left.x) + ","+\
                               str(left.y) + ","+\
                               str(left.z) + ","+\
                               str(left.rx) + ","+\
                               str(left.ry) + ","+\
                               str(left.rz) + ",L"
        elif leftMv == Move.OFFSET_W:
            leftCMD = "OffsetW,"+str(left.x) + ","+\
                                 str(left.y) + ","+\
                                 str(left.z) + ",L"
        elif leftMv == Move.OFFSET_T:
            leftCMD = "OffsetT,"+str(left.x) + ","+\
                                 str(left.y) + ","+\
                                 str(left.z) + ","+\
                                 str(left.rx) + ","+\
                                 str(left.ry) + ","+\
                                 str(left.rz) + ",L"
        
        if rightMv == Move.MOVE_J:
            rightCMD = "MoveJ,"+str(left.x) + ","+\
                               str(left.y) + ","+\
                               str(left.z) + ","+\
                               str(left.rx) + ","+\
                               str(left.ry) + ","+\
                               str(left.rz) + ",R"
        elif rightMv == Move.MOVE_L:
            rightCMD = "MoveL,"+str(left.x) + ","+\
                               str(left.y) + ","+\
                               str(left.z) + ","+\
                               str(left.rx) + ","+\
                               str(left.ry) + ","+\
                               str(left.rz) + ",R"
        elif rightMv == Move.OFFSET_W:
            rightCMD = "OffsetW,"+str(left.x) + ","+\
                                 str(left.y) + ","+\
                                 str(left.z) + ",R"
        elif rightMv == Move.OFFSET_T:
            rightCMD = "OffsetT,"+str(left.x) + ","+\
                                 str(left.y) + ","+\
                                 str(left.z) + ","+\
                                 str(left.rx) + ","+\
                                 str(left.ry) + ","+\
                                 str(left.rz) + ",R"

        msg = "%*7*" + leftCMD + "*" + rightCMD + "*" + rerunPath + "*#"
        msg = msg.encode('utf-8')+b"\0"
        return 

    #左臂复位
    @classmethod
    def resetLArm(cls):
        return b"%*8*Reset,L*#\0"

    #右臂复位
    @classmethod
    def resetRArm(cls):
        return b"%*9*Reset,R*#\0"

    #左臂抓手位移
    @classmethod
    def LHandMove(cls,handWidth):
        cmd = "%*10*HandMove," + str(handWidth) + ",L*#"
        cmd = cmd.encode('utf-8')+b"\0"
        return cmd
    
    #右臂抓手位移
    @classmethod
    def RHandMove(cls,handWidth):
        cmd = "%*11*HandMove," + str(handWidth) + ",R*#"
        cmd = cmd.encode('utf-8')+b"\0"
        return cmd

    #运行完成
    @classmethod
    def moveOK(cls):
        return b"%*80*1*#\0"
    
    #运行出现错误
    @classmethod
    def moveErr(cls):
        return b"%*80*0*#\0"
    
    #获取再现库文件名指令
    @classmethod
    def getRerunFileNames(cls):
        return b"%*90*#\0"

    #运行出现帧错误
    @classmethod
    def frameErr(cls):
        return b"%*100*#\0"

    #运行出现打开示教文件错误
    @classmethod
    def rerunFileErr(cls):
        return b"%*101*#\0"

    #左臂占位符错误
    @classmethod
    def LPlaceholderErr(cls):
        return b"%*102*1*#\0"

    #右臂占位符错误
    @classmethod
    def RPlaceholderErr(cls):
        return b"%*102*2*#\0"

    #双臂占位符错误
    @classmethod
    def BPlaceholderErr(cls):
        return b"%*102*3*#\0"

    #左臂位置是否可达  世界坐标系下
    @classmethod
    def LPosIsReachable(cls,left):
        msg = "%*40*LReach,"+str(left.x) + ","+\
                             str(left.y) + ","+\
                             str(left.z) + ","+\
                             str(left.rx) + ","+\
                             str(left.ry) + ","+\
                             str(left.rz) + ",L*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg

    #左臂位置是否可达 世界坐标系下
    @classmethod
    def RPosIsReachable(cls,right):
        msg = "%*41*RReach,"+str(right.x) + ","+\
                             str(right.y) + ","+\
                             str(right.z) + ","+\
                             str(right.rx) + ","+\
                             str(right.ry) + ","+\
                             str(right.rz) + ",R*#"

        msg = msg.encode('utf-8')+b"\0"
        return msg

    #左臂位置可达 
    @classmethod
    def LPosReachable(cls):
        return b"%*40*LReach*1*L*#\0"

    #左臂位置不可达
    @classmethod
    def LPosUnReachable(cls):
        return b"%*40*LReach*0*L*#\0"

    #右臂位置可达
    @classmethod
    def RPosReachable(cls):
        return b"%*41*RReach*1*R*#\0"

    #右臂位置不可达
    @classmethod
    def RPosUnReachable(cls):
        return b"%*41*RReach*0*R*#\0"
    #序列化
    @classmethod
    def parseMsg(cls,msg):

        if msg == str(cls.pingOK()) :
            return Info.PING_OK , "ping成功"

        if msg == str(cls.serverTcpsOK()) :
            return Info.TCP_OK , "服务端Tcp OK"

        if msg == str(cls.serverTcpsErr()) :
            return Info.TCP_ERR , "服务端Tcp异常"

        if msg == str(cls.serverNotRunning()) :
            return Info.SERVER_NOT_RUNNING , "服务端空闲...."

        if msg == str(cls.serverIsRunning()) :
            return Info.SERVER_IS_RUNNING , "服务端正在运行....."

        if msg == str(cls.moveOK()) :
            return Info.MOVE_OK , "运行成功"

        if msg == str(cls.moveErr()) :
            return Info.MOVE_ERR, "运行失败"

        if msg == str(cls.frameErr()) :
            return Info.FRAME_ERR ,"帧错误"

        if msg == str(cls.rerunFileErr()) :
            return Info.RERUN_FILE_ERR , "再现文件错误"

        if msg == str(cls.LPlaceholderErr()) :
            return Info.L_PLACEHOLDER_ERR , "左臂占位符错误" 
            
        if msg == str(cls.RPlaceholderErr()) :
            return Info.R_PLACEHOLDER_ERR , "右臂占位符错误" 
            
        if msg == str(cls.BPlaceholderErr()) :
            return Info.B_PLACEHOLDER_ERR , "双臂占位符错误" 

        if msg == str(cls.LPosReachable()):
            return Info.L_POS_RECHABLE , "左臂目标可达" 

        if msg == str(cls.LPosUnReachable()):
            return Info.L_POS_UNRECHABLE , "左臂目标不可达"

        if msg == str(cls.RPosReachable()):
            return Info.R_POS_RECHABLE , "右臂目标可达" 

        if msg == str(cls.RPosUnReachable()):
            return Info.R_POS_UNRECHABLE , "右臂目标不可达"

        msg = msg[2:len(msg)-5]

        msgList = msg.split("*")

        if msgList[0] == "%" and msgList[len(msgList)-1] =="#" :
            if msgList[1] == "90" :
                names = msgList[2].split(",")
                return Info.RERUN_FILE_NAMES , names
            else :
                return Info.FRAME_ERR ,"帧错误"
        else:
            return Info.FRAME_ERR ,"帧错误"

